#include "hedmodel.h"
#include "solid.h"
#include "face.h"
#include "halfedge.h"
#include "loop.h"
#include "vertex.h"
#include "edge.h"
#include "point.h"
#include <iostream>
#include <math.h>

#define PI 3.14159265


HEDModel::HEDModel()
:firsts(NULL)
{

}


Solid* 
HEDModel::getFirsts()
{
	return firsts;
}


void 
HEDModel::setFirsts( Solid* _firsts)
{
	firsts = _firsts;
}


Solid*
HEDModel::getsolid(int sn)
{
	Solid* s = NULL;
	for(s = firsts; s != NULL; s = s->getNexts())
	{
		if(s->getSolidno() == sn)
			return(s);
	}

	return (NULL);
}


Face* 
HEDModel::fface(Solid* s, int fn)
{
	Face* f = NULL;
	for(f = s->getSfaces(); f != NULL; f = f->getNextf())
	{
		if(f->getFaceno() == fn)
			return (f);
	}
	return (NULL);
}


HalfEdge* 
HEDModel::fhe(Face* f, int vn1, int vn2)
{
	Loop* l = NULL;
	HalfEdge* h = NULL;
	for(l = f->getFloops(); l != NULL; l = l->getNextl())
	{
		h = l->getLedg();
		do
		{
			if((h->getVtx()->getVertexno() == vn1) && (h->getNxt()->getVtx()->getVertexno() == vn2))
			return (h);
		}
		while((h = h->getNxt()) != l->getLedg());
	}
	return (NULL);
}


Vertex* 
HEDModel::getVertex(int vn)
{
  for(Solid* s = firsts; s != NULL; s = s->getNexts())
  {
    for(Vertex* v = s->getVertex(); v != NULL; v = v->getNextVertex())
    {
      if(v->getVertexno() == vn)
      {
        return v;
      }
    }
  }
  return( NULL );
}




///////////////////////////////////////////////////////////////////////////////
// finds the vertex closest to the coordinate (x,y,z) for the given tolerance
///////////////////////////////////////////////////////////////////////////////
Vertex* 
HEDModel::getVertex(double _x, double _y, double _z, double _tol)
{
  // TODO procurar quais dos vertices esta mais proximo do ponto informado
  double tol2 = _tol*_tol;
  for(Solid* s = firsts; s != NULL; s = s->getNexts())
  {
    for(Vertex* v = s->getVertex(); v != NULL; v = v->getNextVertex())
    {
      Point* pt = v->getPoint();
      if( pt->distance2(_x,_y,_z) < tol2 )
      {
        return v;
      }
    }
  }
  return NULL;
}



///////////////////////////////////////////////////////////////////////////////
// finds the edge which the coordinate (x,y,z) lies in
///////////////////////////////////////////////////////////////////////////////
Edge* 
HEDModel::getEdge(double _x, double _y, double _z, double _tol)
{
  // TODO procurar quais das arestas esta mais proximo do ponto informado
  for(Solid* s = firsts; s != NULL; s = s->getNexts())
  {
    for(Edge* e = s->getEdge(); e != NULL; e = e->getNextEdge())
    {
      Point* pt1 = e->getFirstVertex()->getPoint();
      Point* pt2 = e->getSeconVertex()->getPoint();
      double d0 = sqrt( pt1->distance2(pt2->getX(),pt2->getY(),pt2->getZ()) );
      double d1 = sqrt( pt1->distance2(_x,_y,_z) );
      double d2 = sqrt( pt2->distance2(_x,_y,_z) );
      if( fabs(d1+d2-d0) < 0.06*_tol )
      {
        return e;
      }
    }
  }
  return NULL;
}


///////////////////////////////////////////////////////////////////////////////
// finds the face which the coordinate (x,y,z) lies in
///////////////////////////////////////////////////////////////////////////////
Face* 
HEDModel::getFace(double _x, double _y, double _z, double _tol )
{
  // TODO: first checks if is in the external face (id=1)
  // use bounding boxes first, then afterwards the ray-shooting procedure
  Face* outFace = NULL;
  for(Solid* s = firsts; s != NULL; s = s->getNexts())
  {
    double xinters;
    for(Face* f = s->getFace(); f != NULL; f = f->getNextf())
    {
      int nHE = 0;
      int counter = 0;
      Loop* l = f->getFloops();
      Loop* lout = f->getLoopOut();
      if( lout == NULL ) // if external face
      {
        outFace = f;
        continue;
      }
      HalfEdge* h = lout->getLedg();
      do
      {
        nHE++;
        Point* pt1 = h->getVtx()->getPoint();
        Point* pt2 = h->getNxt()->getVtx()->getPoint();
        int numbV1 = h->getVtx()->getVertexno();
        int numbV2 = h->getNxt()->getVtx()->getVertexno();
        if (_y > min(pt1->getY(),pt2->getY()))
        {
          if (_y <= max(pt1->getY(),pt2->getY())) 
          {
            if (_x <= max(pt1->getX(),pt2->getX())) 
            {
              if (pt1->getY() != pt2->getY()) 
              {
                xinters = (_y-pt1->getY())*(pt2->getX()-pt1->getX())/(pt2->getY()-pt1->getY())+pt1->getX();
                if (pt1->getX() == pt2->getX() || _x <= xinters)
                  counter++;
              }
            }
          }
        }
      }
      while( (h = h->getNxt()) != lout->getLedg() );

      if( counter % 2 != 0 )
      {
        return ( f );
      }
    }
  }
  return outFace;
}


int 
HEDModel::getNumberOfVertexes()
{
  // counts the number of vertexes in the model
  int numVtxs = 0;
  for(Solid* s = firsts; s != NULL; s = s->getNexts())
  {
    for(Vertex* v = s->getVertex(); v != NULL; v = v->getNextVertex())
    {
      numVtxs++;
    }
  }
  return numVtxs;
}


vector<Vertex*> 
HEDModel::getVertexes()
{
  vector<Vertex*> vtxs;
  for(Solid* s = firsts; s != NULL; s = s->getNexts())
  {
    for(Vertex* v = s->getVertex(); v != NULL; v = v->getNextVertex())
    {
      vtxs.push_back( v );
    }
  }
  return vtxs;
}


vector<Edge*> 
HEDModel::getEdges()
{
  vector<Edge*> edges;
  for(Solid* s = firsts; s != NULL; s = s->getNexts())
  {
    for(Edge* e = s->getEdge(); e != NULL; e = e->getNextEdge())
    {
      edges.push_back( e );
    }
  }
  return edges;
}


int 
HEDModel::getNumberOfFaces()
{
  // counts the number of faces in the model
  int numFaces = 0;
  for(Solid* s = firsts; s != NULL; s = s->getNexts())
  {
    for(Face* f = s->getSfaces(); f != NULL; f = f->getNextf())
    {
      numFaces++;
    }
  }
  return numFaces;
}


vector<Face*> 
HEDModel::getFaces()
{
  vector<Face*> faces;
  for(Solid* s = firsts; s != NULL; s = s->getNexts())
  {
    for(Face* f = s->getSfaces(); f != NULL; f = f->getNextf())
    {
      faces.push_back( f );
    }
  }
  return faces;
}


void 
HEDModel::getEdgeVertexes()
{
	std::cout << "HEDModel::getEdgeVertexes nao implementado.\n";
}


void 
HEDModel::replaceVertex()
{
	std::cout << "HEDModel::replaceVertex nao implementado.\n";
}


void 
HEDModel::getCurve()
{
	std::cout << "HEDModel::replaceVertex nao implementado.\n";
}



HalfEdge* 
HEDModel::getHalfEdge( Vertex* _v, double _x, double _y, double _z )
{
  Edge* e;
  Edge* e1;
  Edge* e2;
  double angle1, angle2, anglep;
  double deltaX1, deltaY1, deltaX2, deltaY2, deltaXp, deltaYp;
  double difference1, difference2;
  bool firstEdge = true;
  bool halfEdge1 = false;
  bool halfEdge2 = false;
  // if the solid has no edge then it has just its first half-edge
  if( firsts->getEdge() != NULL )
  {
    e = firsts->getEdge();
    e1 = NULL;
    do
    {
      if( ((e->getHe1()->getVtx()==_v) || (e->getHe2()->getVtx()==_v)) && firstEdge )
      {
        e1 = e;
        if( e->getHe1()->getVtx() == _v )
        {
          halfEdge1 = true;
        }
        firstEdge = false;
      }
      else if( (e->getHe1()->getVtx()==_v) || (e->getHe2()->getVtx()==_v) )
      {
        e2 = e;
        if( e->getHe1()->getVtx() == _v )
        {
          halfEdge2 = true;
        }
        if( halfEdge1 )
        {
          Point* p1 = e1->getHe1()->getVtx()->getPoint();
          Point* p2 = e1->getHe2()->getVtx()->getPoint();
          deltaX1 = p2->getX() - p1->getX();
          deltaY1 = p2->getY() - p1->getY();
        }
        else
        {
          Point* p1 = e1->getHe1()->getVtx()->getPoint();
          Point* p2 = e1->getHe2()->getVtx()->getPoint();
          deltaX1 = p1->getX() - p2->getX();
          deltaY1 = p1->getY() - p2->getY();
        }
        if( halfEdge2 )
        {
          Point* p1 = e2->getHe1()->getVtx()->getPoint();
          Point* p2 = e2->getHe2()->getVtx()->getPoint();
          deltaX2 = p2->getX() - p1->getX();
          deltaY2 = p2->getY() - p1->getY();
        }
        else
        {
          Point* p1 = e2->getHe1()->getVtx()->getPoint();
          Point* p2 = e2->getHe2()->getVtx()->getPoint();
          deltaX2 = p1->getX() - p2->getX();
          deltaY2 = p1->getY() - p2->getY();
        }
        deltaXp = _x - _v->getPoint()->getX();
        deltaYp = _y - _v->getPoint()->getY();
        if( deltaX1 != 0 )
        {
          angle1 = atan(deltaY1/deltaX1);
          if( deltaX1 < 0 )
            angle1 = angle1 + PI;
          else if(deltaY1<0)
            angle1 = angle1 + PI*2;
        }
        else if( deltaY1 > 0 )
        {
          angle1 =PI/2;
        }
        else
        {
          angle1 = PI*3/2;
        }
        if(deltaX2!=0)
        {
          angle2 = atan(deltaY2/deltaX2);
          if( deltaX2 < 0 )
            angle2 = angle2 + PI;
          else if( deltaY2 < 0 )
            angle2 = angle2 + PI*2;
        }
        else if( deltaY2 > 0 )
        {
          angle2 = PI/2;
        }
        else
        {
          angle2 = PI*3/2;
        }
        if( deltaXp != 0 )
        {
          anglep = atan(deltaYp/deltaXp);
          if( deltaXp < 0 )
            anglep = anglep + PI;
          else if( deltaYp < 0 )
            anglep = anglep + PI*2;
        }
        else if( deltaYp > 0 )
        {
          anglep =	PI/2;
        }
        else
        {
          anglep = PI*3/2;
        }
        difference1 = anglep - angle1;
        difference2 = anglep - angle2;
        if( (difference1>0&&difference2>0) || (difference1<0&&difference2<0) )
        {
          if( difference2 > difference1 )
          {
            e1 = e2;
            if(halfEdge2)
              halfEdge1 = true;
            else
              halfEdge1 = false;
          }
        }
        else
        {
          if( difference2 < 0 )
          {
            e1 = e2;
            if( halfEdge2 )
              halfEdge1 = true;
            else
              halfEdge1 = false;
          }
        }
        halfEdge2=false;
      }
    }
    while( (e=e->getNextEdge()) != NULL );

    if( e1 != NULL )
    {
      if( e1->getHe1()->getVtx() == _v )
      {
        return e1->getHe1();
      }
      else
      {
        return e1->getHe2();
      }
    }
    else
    {
      return _v->getVEdge();
    }
  }
  else
  {
    return _v->getVEdge();
  }
}




HalfEdge* 
HEDModel::getHalfEdgeNEW( Vertex* _v, double _x, double _y, double _z )
{
  HalfEdge* he_i = _v->getVEdge();
  if( he_i->getEdg() != NULL )
  {
    // counts how many edges this vertex contains:
    int nEdges = 1;
    while( (he_i= he_i->mate()->getNxt()) != _v->getVEdge() )
      nEdges++;
    // case the vertex contains only one edge then returns its half-edge,
    // otherwise returns the half-edge that is most right of the "new edge"
    if( nEdges > 1 )
    {
      // computes the angle with the horizontal for the "new edge"
      double deltaX = _x - _v->getPoint()->getX();
      double deltaY = _y - _v->getPoint()->getY();
      double angleRef = 0.0;
      double angle_min = 2.0*PI;
      HalfEdge* he_min = NULL;
      if( deltaX != 0 )  // different of 90 and 270 degrees
      {
        angleRef = atan(deltaY/deltaX); 
        // correct angle (to positive value) in case a negative dimension
        if( deltaX < 0 )
          angleRef = angleRef + PI;
        else if( deltaY < 0 )
          angleRef = angleRef + PI*2;
      }
      else if( deltaY > 0 )  // 90 degrees
        angleRef = PI/2.0;
      else  // 270 degrees
        angleRef = PI*3.0/2.0;
      //std::cout << "ANG REF = " << angleRef << std::endl;
      // loops over the vertex edges to identify the desired half-edge
      he_i = _v->getVEdge();
      do
      {
        // computes the angle with the horizontal for the "current edge"
        double deltaX_i = he_i->mate()->getVtx()->getPoint()->getX() - _v->getPoint()->getX();
        double deltaY_i = he_i->mate()->getVtx()->getPoint()->getY() - _v->getPoint()->getY();
        double angle_i = 0.0;
        if( deltaX_i != 0 )  // different of 90 and 270 degrees
        {
          angle_i = atan(deltaY_i/deltaX_i); 
          // correct angle (to positive value) in case a negative dimension
          if( deltaX_i < 0 )
            angle_i = angle_i + PI;
          else if( deltaY_i < 0 )
            angle_i = angle_i + PI*2;
        }
        else if( deltaY_i > 0 )  // 90 degrees
          angle_i = PI/2.0;
        else  // 270 degrees
          angle_i = PI*3.0/2.0;
        //std::cout << "ANG i = " << angle_i << std::endl;
        // obtains only positive values from reference edge in ccw 
        angle_i = angleRef - angle_i;
        if( angle_i < 0 )
          angle_i = angle_i + 2.0*PI;
        if( angle_i < angle_min )
        {
          angle_min = angle_i;
          he_min = he_i;
        }
      }
      while ( (he_i= he_i->mate()->getNxt()) != _v->getVEdge() );
      return he_min;
    } 
    else
    {
      return he_i;
    }
  }
  else
  {
    return he_i;
  }
  return he_i;
}





// solution from [SKIENA & REVILLA, 2002. Programming Challenges, p.319]
void HEDModel::triagulate(vector<Point*>& p, vector<vector<int> >& t)
{
//  triangulate(polygon *p, triangulation *t)
  {
    int counter = 0;
    int pn = p.size();
    int l[50], r[50]; /* left/right neighbor indices */
    int i; /* counter */
    for( i=0; i<pn; i++ ) 
    { /* initialization */
      l[i] = ((i-1) + pn) % pn;
      r[i] = ((i+1) + pn) % pn;
    }
    i = pn-1;
    while(t.size() < (pn-2)) 
    {
      i = r[i];
      if (ear_Q(l[i],i,r[i],p)) 
      {
        vector<int> tri(3);
        tri[0] = l[i];
        tri[1] = i;
        tri[2] = r[i];
        t.push_back( tri );
        //add_triangle(t,l[i],i,r[i],p);
        l[ r[i] ] = l[i];
        r[ l[i] ] = r[i];
      }
      // stops in case it has problems with tolerance:
      if(++counter>100*pn)
        break;
    }
  }
}


bool HEDModel::ear_Q(int i, int j, int k, vector<Point*>& p)
{
   vector<Point*> t(3); /* coordinates for points i,j,k */
   int m; /* counter */
   t[0] = p[i];
   t[1] = p[j];
   t[2] = p[k];
   //copy_point(p->p[i],t[0]);
   //copy_point(p->p[j],t[1]);
   //copy_point(p->p[k],t[2]);
   if (cw(t[0],t[1],t[2])) 
     return (false);
   for (m=0; m<p.size(); m++) 
   {
     if ((m!=i) && (m!=j) && (m!=k))
       if (point_in_triangle(p[m],t)) 
         return (false);
   }
  return (true);
}

 bool HEDModel::point_in_triangle(Point* p, vector<Point*>& t)
 {
   int i; /* counter */
   for (i=0; i<3; i++)
     if (cw(t[i],t[(i+1)%3],p)) 
       return(false);
   return(true);
 }

 bool HEDModel::cw(Point* a, Point* b, Point* c)
 {
   return (signed_triangle_area(a,b,c) < 1.0e-5);
 }

 double HEDModel::signed_triangle_area(Point* a, Point* b, Point* c)
 {
   return( (a->getX()*b->getY() - a->getY()*b->getX() + a->getY()*c->getX()
   - a->getX()*c->getY() + b->getX()*c->getY() - c->getX()*b->getY()) / 2.0 );
 }

